#include "encoder.h"
/*  one circle 2380  */
double encoder_data_f_l = 0;
double encoder_data_f_r = 0;
double encoder_data_b_l = 0;
double encoder_data_b_r = 0;

void encoder_init()
{
    encoder_quad_init(ENCODER_DIR_F_L, ENCODER_QUADDEC_A_F_L, ENCODER_DIR_B_F_L);
    encoder_quad_init(ENCODER_DIR_F_R, ENCODER_QUADDEC_A_F_R, ENCODER_DIR_B_F_R);
    encoder_quad_init(ENCODER_DIR_B_L, ENCODER_QUADDEC_A_B_L, ENCODER_DIR_B_B_L);
    encoder_quad_init(ENCODER_DIR_B_R, ENCODER_QUADDEC_A_B_R, ENCODER_DIR_B_B_R);
}

void print_encoder_data()
{
    printf("encoder_data_f_l = %f  ", encoder_data_f_l);
    printf("encoder_data_f_r = %f  ", encoder_data_f_r);
    printf("encoder_data_b_l = %f  ", encoder_data_b_l);
    printf("encoder_data_b_r = %f  ", encoder_data_b_r);
    printf("speed-avg = %f  \r\n", (encoder_data_f_l + encoder_data_f_r + encoder_data_b_l + encoder_data_b_r) / 4);
}
